module cordic_datapath
#(
  parameter DATA_WIDTH = 16,
  parameter COUNT_SIZE = 4
)(
  //OUTPUTS
  output signed [DATA_WIDTH - 1:0] x_out,
  output signed [DATA_WIDTH - 1:0] y_out,
  //output signed [DATA_WIDTH - 1:0] teta_out,
  output        [COUNT_SIZE - 1:0] count,
  output ready, 
  //INPUTS
  input signed [DATA_WIDTH - 1:0] x_in,
  input signed [DATA_WIDTH - 1:0] y_in,
  input signed [DATA_WIDTH - 1:0] teta_in,
  input signed [DATA_WIDTH - 1:0] arc_tg,
  input start,
  input reg_en,
  input reg_src,
  input count_clear,
  input count_en,
  input clk,
  input rst_n
);

wire signed [DATA_WIDTH - 1:0] x, x_core;
wire signed [DATA_WIDTH - 1:0] y, y_core;
wire signed [DATA_WIDTH - 1:0] teta, teta_core, teta_i;

wire regx_en, regy_en, teta_en;
//wire regx_sel, regy_sel, teta_sel;
wire msb_teta;

wire signed [DATA_WIDTH - 1:0] x_sft, y_sft, zero_in;
wire teta_dif;

assign regx_en = (start | reg_en);
assign regy_en = (start | reg_en);
assign teta_en = (start | reg_en);

assign regx_src = reg_src; 
assign regy_src = reg_src;
assign teta_src = reg_src;

assign msb_teta = teta[DATA_WIDTH - 1];
assign teta_dif = ~(teta_in[DATA_WIDTH - 1] ^ teta_in[DATA_WIDTH - 2]);
 
ff_mux_2x1
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  REG_X
(  
  .q       ( x        ),
  .in1     ( x_in     ),
  .in2     ( x_core   ),
  .ff_en   ( regx_en  ),
  .mux_sel ( regx_src ),
  .clk     ( clk      ),
  .rst_n   ( rst_n    )
);

right_shifter
#(
  .DATA_WIDTH ( DATA_WIDTH ),
  .SHIFT_BITS ( COUNT_SIZE )
)
  SHIFT_X
(
  .out   ( x_sft ),
  .in    ( x     ),
  .shift ( count )
);

add_sub
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  ADDSUB_X
(
  .result    ( x_core   ),
  .op_add    ( x        ),
  .op_sub    ( y_sft    ),
  .operation ( msb_teta )
);

ff_mux_2x1
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  REG_Y
(  
  .q       ( y        ),
  .in1     ( y_in     ),
  .in2     ( y_core   ),
  .ff_en   ( regy_en  ),
  .mux_sel ( regy_src ),
  .clk     ( clk      ),
  .rst_n   ( rst_n    )
);

right_shifter
#(
  .DATA_WIDTH ( DATA_WIDTH ),
  .SHIFT_BITS ( COUNT_SIZE )
)
  SHIFT_Y
(
  .out   ( y_sft ),
  .in    ( y     ),
  .shift ( count )
);

add_sub
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  ADDSUB_Y
(
  .result    ( y_core    ),
  .op_add    ( y         ),
  .op_sub    ( x_sft     ),
  .operation ( ~msb_teta )
);

assign zero_in = {DATA_WIDTH{1'b0}};

add_sub
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  ADDSUB_INV_X
(
  .result    ( x_out    ),
  .op_add    ( zero_in  ),
  .op_sub    ( x        ),
  .operation ( teta_dif )
);

add_sub
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  ADDSUB_INV_Y
(
  .result    ( y_out    ),
  .op_add    ( zero_in  ),
  .op_sub    ( y        ),
  .operation ( teta_dif )
);

assign teta_i = {teta_in[DATA_WIDTH - 2], teta_in[DATA_WIDTH - 2:0]};
//assign teta_i = teta_in;
ff_mux_2x1
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  REG_TETA
(  
  .q       ( teta      ),
  .in1     ( teta_i    ),
  .in2     ( teta_core ),
  .ff_en   ( teta_en   ),
  .mux_sel ( teta_src  ),
  .clk     ( clk       ),
  .rst_n   ( rst_n     )
);

add_sub
#(
  .DATA_WIDTH ( DATA_WIDTH )
)
  ADDSUB_TETA
(
  .result    ( teta_core ),
  .op_add    ( teta      ),
  .op_sub    ( arc_tg    ),
  .operation ( msb_teta  )
);

counter_clear 
#(
  .COUNT_WIDTH( COUNT_SIZE )
)
COUNTER
(
  .count       ( count       ),
  .count_clear ( count_clear ),
  .count_en    ( count_en    ),
  .clk         ( clk         ),
  .rst_n       ( rst_n       )
);

assign ready = (count == {COUNT_SIZE{1'b1}});

endmodule

//-----------------------------------------------------------------------------
//                            MODULES DEFIITIONS
//-----------------------------------------------------------------------------

//=============================================================================
// 1)  Register with MUX 2x1 Definition
//     mux_sel = 1 -> in1
//             = 0 -> in2
//=============================================================================
module ff_mux_2x1
#(
  parameter DATA_WIDTH  = 32,         //FF data width
  parameter RESET_VALUE = 0           //FF value after reset
)(
  //OUTPUTS
  output reg [DATA_WIDTH - 1:0] q,    //FF output
  //INPUTS
  input wire [DATA_WIDTH - 1:0] in1,  //input to MUX
  input wire [DATA_WIDTH - 1:0] in2,  //input to MUX
  input wire ff_en,                   //enable writing in FF
  input wire mux_sel,                 //Select input to FF
  input wire clk,                     //clock
  input wire rst_n                    //reset, asynchrounous and active low
);

wire [DATA_WIDTH - 1:0] mux_out;

assign mux_out = mux_sel ? in1 : in2;

always @(posedge clk, negedge rst_n)
  if(!rst_n)
    q <= RESET_VALUE;
  else if(ff_en)
    q <= mux_out;
      
endmodule

//=============================================================================
// 2)  Counter with clear signal Definition
//=============================================================================
module counter_clear
#(
  parameter COUNT_WIDTH = 4 
)(
  //OUTPUTS
  output reg [COUNT_WIDTH - 1:0] count,   //Counter Output
  //INPUTS
  input count_en,                         //Enables the counter
  input count_clear,                      //Clear counter: when high the counter is restarted
  input clk,                              //Counter clock
  input rst_n                             //reset: asynchrounous, active low
);
  
  always @(posedge clk, negedge rst_n)
    begin
      if(!rst_n)
        count <= {COUNT_WIDTH{1'b0}};
      else
        begin
          if(count_clear)
            count <= {COUNT_WIDTH{1'b0}};
          else
            if(count_en)
              count <= count + 1;
        end
    end
endmodule

//=============================================================================
// 3)  Signed Addition/Subtraction Unit
//     operation = 1 -> addition
//                 0 -> subtraction (op1_add - op_sub)
//=============================================================================
module add_sub
#(
  parameter DATA_WIDTH = 16 
)(
  //OUTPUTS
  output signed [DATA_WIDTH -1:0] result,       //Result of add/sub operation
  //INPUTS
  input signed [DATA_WIDTH -1:0] op_add,        //Operand 1
  input signed [DATA_WIDTH -1:0] op_sub,        //Operand 2
  input operation                               //Select Addition or subtraction operation
);

assign result = (operation) ? op_add + op_sub : op_add - op_sub;

endmodule

//=============================================================================
// 4)  Signed Right Shifter Unit
//=============================================================================
module right_shifter
#(
  parameter DATA_WIDTH = 16,
  parameter SHIFT_BITS = 4 
)(
  //OUTPUTS
  output signed [DATA_WIDTH -1:0]out,         //out = in * 2^(-shift)
  //INPUTS
  input signed [DATA_WIDTH -1:0] in,          //number to be right shifted
  input [SHIFT_BITS - 1:0] shift              //number of shift
);

assign out = in >>> shift;

endmodule